/**=============================================================================
@file
   qhdsp_hvx_fft_common.c

@brief
   Common routines for complex and real HVX implementations of FFT and IFFT.

Copyright (c) 2020 Qualcomm Technologies Incorporated.
All Rights Reserved. Qualcomm Proprietary and Confidential.
=============================================================================**/

#include <stdint.h>
#include "qhdsp_hvx_common.h"
#include "qhcomplex.h"
#include "qhmath.h"
#include "hvx_fft_common.h"
#include "qhdsp_hvx_fft_internal.h"

/**
 * @brief           [HVX] Generate twiddle factors for [fixed-point] complex 1D FFT/IFFT functions.
 * @param[in,out]   twiddle - output buffer for storing generated twiddle factors
 * @param[in]       NP - number of points on which FFT/IFFT is performed
 * @param[in]       log2NP - logarithm of power 2 of number of points
 * @return
 * @note
 *                  - Generated twiddle factors are used for both FFT and IFFT.
 *                  - Code doesn't utilize HVX vector instructions/parallelism.
 *                  - twiddle factors format: Q1.14
 *                  - Assumptions:
 *                                  1. twiddle - buffer aligned by VLENbytes
 *                                  2. twiddle buffer size is NP * [4bytes]
 */
void qhdsp_hvx_fft_gen_twiddles_complex_ach(int32_t* twiddle, uint32_t NP, uint32_t log2NP)
{
    uint32_t i, k, k1, k2;

    float complex x[NP / 2 + 1];
    float complex y[NP / 4];
    int32_t fixedPointValue;

    float ar, ai;

    // Generate twiddles
    // arrange in bit reversed order

    // 1st stage - calculate float point values
    for (i = 0; i <= NP / 2; i++)
    {
        k = bitrev(i, log2NP - 1);
        ar =  qhmath_cos_f((double)(k) * 2.0 * M_PI / (double)NP);
        ai = -qhmath_sin_f((double)(k) * 2.0 * M_PI / (double)NP);
        x[i] = ar + 1i * ai;
    }

    for (i = 0; i < NP / 4; i++)
    {
        k1 = bitrev((2 * i)+1, log2NP - 1);
        k2 = bitrev(i, log2NP - 1);
        k = k1 + k2;
        ar =  qhmath_cos_f((double)(k) * 2.0 * M_PI / (double)NP);
        ai = -qhmath_sin_f((double)(k) * 2.0 * M_PI / (double)NP);
        y[i] = ar + 1i * ai;
    }

    // 2nd stage - convert float point values to 16-bit fixed-point values and pack real&imag parts of twiddle factors into one 32bit word - Q1.14 (Q15 / 2.0)
    for (i = 0; i < NP/4; i++)
    {
        twiddle[4*i + 0] = 0x00004000;

        fixedPointValue = create_complex(   (int16_t)(__real__(x[2*i+1])*32768.0/2.0), \
                                            (int16_t)(__imag__(x[2*i+1])*32768.0/2.0));
        twiddle[4*i + 1] = fixedPointValue;

        fixedPointValue = create_complex(   (int16_t)(__real__(x[i])*32768.0/2.0), \
                                            (int16_t)(__imag__(x[i])*32768.0/2.0));
        twiddle[4*i + 2] = fixedPointValue;

        fixedPointValue = create_complex(   (int16_t)(__real__(y[i])*32768.0/2.0), \
                                            (int16_t)(__imag__(y[i])*32768.0/2.0));
        twiddle[4*i + 3] = fixedPointValue;
    }
}

/**
 * @brief           [HVX] Generate twiddle factors for [fixed-point] real 1D FFT/IFFT functions.
 * @param[in,out]   twiddle1 - output buffer for storing generated twiddle factors - for N/2-point complex FFT
 * @param[in,out]   twiddle2 - output buffer for storing generated twiddle factors - for last stage
 * @param[in]       NP - number of points on which FFT/IFFT is performed
 * @param[in]       log2NP - logarithm of power 2 of number of points
 * @note
 *                  - Generated twiddle factors are used for both FFT and IFFT.
 *                  - Code doesn't utilize HVX vector instructions/parallelism.
 *                  - twiddle1 factors format: Q1.14
 *                  - twiddle2 factors format: Q15
 *                  - Assumptions:
 *                                  1. twiddle1 & twiddle2 - buffers aligned by VLENbytes
 *                                  2. twiddle1 & twiddle2 - buffer size is NP * [4bytes]
 */
void qhdsp_hvx_fft_gen_twiddles_real_ach(int32_t* twiddle1, int32_t* twiddle2, uint32_t NP, uint32_t log2NP)
{
    uint32_t i, k, k1, k2;

    NP     = NP / 2;
    log2NP = log2NP - 1;

    float complex x[NP / 2 + 1];
    float complex y[NP / 4];
    int32_t fixedPointValue;

    float ar, ai;

    // Generate twiddles
    // arrange in bit reversed order

    // 1st stage - calculate float point values
    for (i = 0; i <= NP/2; i++)
    {
        k = bitrev(i, log2NP - 1);
        ar =  qhmath_cos_f((double)(k) * 2.0 * M_PI / (double)NP);
        ai = -qhmath_sin_f((double)(k) * 2.0 * M_PI / (double)NP);
        x[i] = ar + 1i * ai;
    }

    for (i = 0; i < NP / 4; i++)
    {
        k1 = bitrev((2 * i)+1, log2NP - 1);
        k2 = bitrev(i, log2NP - 1);
        k = k1 + k2;
        ar =  qhmath_cos_f((double)(k) * 2.0 * M_PI / (double)NP);
        ai = -qhmath_sin_f((double)(k) * 2.0 * M_PI / (double)NP);
        y[i] = ar + 1i * ai;
    }

    // 2nd stage - convert float point values to 16-bit fixed-point values and pack real&imag parts of twiddle factors into one 32bit word
    for (i = 0; i < NP/4; i++)
    {
        twiddle1[4*i + 0] = 0x00004000;

        fixedPointValue = create_complex(   (int16_t)(__real__(x[2*i+1])*32768.0/2.0), \
                                            (int16_t)(__imag__(x[2*i+1])*32768.0/2.0));
        twiddle1[4*i + 1] = fixedPointValue;

        fixedPointValue = create_complex(   (int16_t)(__real__(x[i])*32768.0/2.0), \
                                            (int16_t)(__imag__(x[i])*32768.0/2.0));
        twiddle1[4*i + 2] = fixedPointValue;

        fixedPointValue = create_complex(   (int16_t)(__real__(y[i])*32768.0/2.0), \
                                            (int16_t)(__imag__(y[i])*32768.0/2.0));
        twiddle1[4*i + 3] = fixedPointValue;
    }

    for (i = 1; i <= NP; i++)
    {
        ar = qhmath_sin_f((double)(i) * M_PI / (double)NP);
        ai = qhmath_cos_f((double)(i) * M_PI / (double)NP);

        fixedPointValue = create_complex(   (ar == (float)1.0)?(int16_t)(0x7fff):(int16_t)(ar*32768.0)   ,  \
                                            (ai == (float)1.0)?(int16_t)(0x7fff):(int16_t)(ai*32768.0)   );

        twiddle2[i - 1] = fixedPointValue;
    }
}

/**
 * @brief           [HVX] Generate twiddle factors for [fixed-point] real 2D FFT/IFFT functions.
 * @param[in,out]   twiddle1 - output buffer for storing generated twiddle factors - for N/2-point complex FFT
 * @param[in,out]   twiddle2 - output buffer for storing generated twiddle factors - for last stage
 * @param[in]       NP - number of points on which FFT/IFFT is performed
 * @param[in]       log2NP - logarithm of power 2 of number of points
 * @note
 *                  - Generated twiddle factors are used for both FFT and IFFT.
 *                  - Code doesn't utilize HVX vector instructions/parallelism.
 *                  - twiddle1 factors format: Q1.14
 *                  - twiddle2 factors format: Q15
 *                  - Assumptions:
 *                                  1. twiddle1 & twiddle2 - buffers aligned by VLENbytes
 *                                  2. twiddle1 - buffer size is NP * [4bytes]
 *                                  2. twiddle2 - buffer size is NP/2 * [4bytes]
 */
void qhdsp_hvx_2D_fft_gen_twiddles_real_ach(int32_t* twiddle1, int32_t* twiddle2, uint32_t NP, uint32_t log2NP)
{
    uint32_t i, k, k1, k2;

    NP     = NP / 2;
    log2NP = log2NP - 1;

    float complex x[NP + 1];
    float complex y[NP / 2];
    int32_t fixedPointValue;

    float ar, ai;

    // Generate twiddles
    // arrange in bit reversed order

    // 1st stage - calculate float point values
    for (i = 0; i <= NP; i++)
    {
        k = bitrev(i, log2NP - 1);
        ar =  qhmath_cos_f((double)(k) * 2.0 * M_PI / (double)NP);
        ai = -qhmath_sin_f((double)(k) * 2.0 * M_PI / (double)NP);
        x[i] = ar + 1i * ai;
    }

    for (i = 0; i < NP / 2; i++)
    {
        k1 = bitrev((2 * i)+1, log2NP - 1);
        k2 = bitrev(i, log2NP - 1);
        k = k1 + k2;
        ar =  qhmath_cos_f((double)(k) * 2.0 * M_PI / (double)NP);
        ai = -qhmath_sin_f((double)(k) * 2.0 * M_PI / (double)NP);
        y[i] = ar + 1i * ai;
    }

    // 2nd stage - convert float point values to 16-bit fixed-point values and pack real&imag parts of twiddle factors into one 32bit word
    for (i = 0; i < NP/2; i++)
    {
        twiddle1[4*i + 0] = 0x00004000;

        fixedPointValue = create_complex(   (int16_t)(__real__(x[2*i+1])*32768.0/2.0), \
                                            (int16_t)(__imag__(x[2*i+1])*32768.0/2.0));
        twiddle1[4*i + 1] = fixedPointValue;

        fixedPointValue = create_complex(   (int16_t)(__real__(x[i])*32768.0/2.0), \
                                            (int16_t)(__imag__(x[i])*32768.0/2.0));
        twiddle1[4*i + 2] = fixedPointValue;

        fixedPointValue = create_complex(   (int16_t)(__real__(y[i])*32768.0/2.0), \
                                            (int16_t)(__imag__(y[i])*32768.0/2.0));
        twiddle1[4*i + 3] = fixedPointValue;
    }

    for (i = 1; i <= NP; i++)
    {
        ar = qhmath_sin_f((double)(i) * M_PI / (double)NP);
        ai = qhmath_cos_f((double)(i) * M_PI / (double)NP);

        fixedPointValue = create_complex(   (ar == (float)1.0)?(int16_t)(0x7fff):(int16_t)(ar*32768.0)   ,  \
                                            (ai == (float)1.0)?(int16_t)(0x7fff):(int16_t)(ai*32768.0)   );

        twiddle2[i - 1] = fixedPointValue;
    }
}

#if __HVX_ARCH__ >= 68

/**
 * @brief           [HVX] Generate twiddle factors for [float-point] complex 1D FFT/IFFT functions.
 * @param[in,out]   twiddle - output buffer for storing generated twiddle factors
 * @param[in]       NP - number of points on which FFT/IFFT is performed
 * @param[in]       log2NP - logarithm of power 2 of number of points
 * @return
 * @note
 *                  - Generated twiddle factors are used for both FFT and IFFT.
 *                  - Code doesn't utilize HVX vector instructions/parallelism.
 *                  - Assumptions:
 *                                  1. twiddle - buffer aligned by VLENbytes
 *                                  2. twiddle buffer size is NP * [8bytes]
 */
void qhdsp_hvx_fft_gen_twiddles_complex_acf(float complex* twiddle, uint32_t NP, uint32_t log2NP)
{
    uint32_t i, k, k1, k2;
    float complex x[NP / 2];
    float complex y[NP / 4];

    float ar, ai;

    // Generate twiddles
    // arrange in bit reversed order

    for (i = 0; i < NP / 2; i++)
    {
        k = bitrev(i, log2NP - 1);
        ar =  qhmath_cos_f((double)(k) * 2.0 * M_PI / (double)NP);
        ai = -qhmath_sin_f((double)(k) * 2.0 * M_PI / (double)NP);
        x[i] = ar + 1i * ai;
    }

    for (i = 0; i < NP / 4; i++)
    {
        k1 = bitrev((2 * i), log2NP - 1);
        k2 = bitrev(i, log2NP - 1);
        k = k1 + k2;
        ar =  qhmath_cos_f((double)(k) * 2.0 * M_PI / (double)NP);
        ai = -qhmath_sin_f((double)(k) * 2.0 * M_PI / (double)NP);
        y[i] = ar + 1i * ai;
    }

    for (i = 0; i < NP / 4; i++)
    {
        twiddle[4 * i + 0] = 1.0;
        twiddle[4 * i + 1] = x[2 * i];
        twiddle[4 * i + 2] = x[i];
        twiddle[4 * i + 3] = y[i];
    }
}

/**
 * @brief           [HVX] Generate twiddle factors for [float-point] real 1D FFT/IFFT functions.
 * @param[in,out]   twiddle1 - output buffer for storing generated twiddle factors - for N/2-point complex FFT
 * @param[in,out]   twiddle2 - output buffer for storing generated twiddle factors - for last stage
 * @param[in]       NP - number of points on which FFT/IFFT is performed
 * @param[in]       log2NP - logarithm of power 2 of number of points
 * @note
 *                  - Generated twiddle factors are used for both FFT and IFFT.
 *                  - Code doesn't utilize HVX vector instructions/parallelism.
 *                  - Assumptions:
 *                                  1. twiddle1 & twiddle2 - buffers aligned by VLENbytes
 *                                  2. twiddle1 & twiddle2 - buffer size is NP * [8bytes]
 */
void qhdsp_hvx_fft_gen_twiddles_real_acf(float complex *twiddle1, float complex *twiddle2, uint32_t np, uint32_t log2np)
{
    uint32_t i, k, k1, k2;
    float ar, ai;

    np     = np / 2;
    log2np = log2np - 1;

    // Generate twiddles for complexFFT
    // arrange in bit reversed order
    for (i = 0; i < np / 4; i++)
    {
        twiddle1[4 * i + 0] = 1.0;

        k2 = bitrev(2 * i, log2np - 1);
        ar =  qhmath_cos_f((double)(k2) * 2.0 * M_PI / (double)np);
        ai = -qhmath_sin_f((double)(k2) * 2.0 * M_PI / (double)np);
        twiddle1[4 * i + 1] = ar + 1i * ai;

        k1 = bitrev(i, log2np - 1);
        ar =  qhmath_cos_f((double)(k1) * 2.0 * M_PI / (double)np);
        ai = -qhmath_sin_f((double)(k1) * 2.0 * M_PI / (double)np);
        twiddle1[4 * i + 2] = ar + 1i * ai;

        k = k1 + k2;
        ar =  qhmath_cos_f((double)(k) * 2.0 * M_PI / (double)np);
        ai = -qhmath_sin_f((double)(k) * 2.0 * M_PI / (double)np);
        twiddle1[4 * i + 3] = ar + 1i * ai;
    }

    for (i = 1; i <= np; i++)
    {
        ar = -0.5 * qhmath_sin_f((double)(i) * M_PI / (double)np);
        ai = -0.5 * qhmath_cos_f((double)(i) * M_PI / (double)np);
        twiddle2[i - 1] = ar + 1i * ai;
    }
}

/**
 * @brief           [HVX] Generate twiddle factors for [float-point] real 2D FFT/IFFT functions.
 * @param[in,out]   twiddle1 - output buffer for storing generated twiddle factors - for N/2-point complex FFT
 * @param[in,out]   twiddle2 - output buffer for storing generated twiddle factors - for last stage
 * @param[in]       NP - number of points on which FFT/IFFT is performed
 * @param[in]       log2NP - logarithm of power 2 of number of points
 * @note
 *                  - Generated twiddle factors are used for both FFT and IFFT.
 *                  - Code doesn't utilize HVX vector instructions/parallelism.
 *                  - Assumptions:
 *                                  1. twiddle1 & twiddle2 - buffers aligned by VLENbytes
 *                                  2. twiddle1 - buffer size is NP * [8bytes]
 *                                  2. twiddle2 - buffer size is NP/2 * [8bytes]
 */
void qhdsp_hvx_2D_fft_gen_twiddles_real_acf(float complex* twiddle1, float complex* twiddle2, uint32_t NP, uint32_t log2NP)
{
    uint32_t i, k, k1, k2;
    float complex x[NP / 2];
    float complex y[NP / 4];

    float ar, ai;

    // Generate twiddles
    // arrange in bit reversed order

    for (i = 0; i < NP / 2; i++)
    {
        k = bitrev(i, log2NP - 1);
        ar =  qhmath_cos_f((double)(k) * 2.0 * M_PI / (double)NP);
        ai = -qhmath_sin_f((double)(k) * 2.0 * M_PI / (double)NP);
        x[i] = ar + 1i * ai;
    }

    for (i = 0; i < NP / 4; i++)
    {
        k1 = bitrev((2 * i), log2NP - 1);
        k2 = bitrev(i, log2NP - 1);
        k = k1 + k2;
        ar =  qhmath_cos_f((double)(k) * 2.0 * M_PI / (double)NP);
        ai = -qhmath_sin_f((double)(k) * 2.0 * M_PI / (double)NP);
        y[i] = ar + 1i * ai;
    }

    for (i = 0; i < NP / 4; i++)
    {
        twiddle1[4 * i + 0] = 1.0;
        twiddle1[4 * i + 1] = x[2 * i];
        twiddle1[4 * i + 2] = x[i];
        twiddle1[4 * i + 3] = y[i];
    }

    NP = NP/2;

    for (i = 1; i <= NP; i++)
    {
        ar = -0.5 * qhmath_sin_f((double)(i) * M_PI / (double)NP);
        ai = -0.5 * qhmath_cos_f((double)(i) * M_PI / (double)NP);
        twiddle2[i - 1] = ar + 1i * ai;
    }
}

/**
 * @brief           [HVX] Generate twiddle factors for [half precision float-point] complex 1D FFT/IFFT functions.
 * @param[in,out]   twiddle - output buffer for storing generated twiddle factors
 * @param[in]       NP - number of points on which FFT/IFFT is performed
 * @param[in]       log2NP - logarithm of power 2 of number of points
 * @return
 * @note
 *                  - Generated twiddle factors are used for both FFT and IFFT.
 *                  - Code doesn't utilize HVX vector instructions/parallelism.
 *                  - Assumptions:
 *                                  1. twiddle - buffer aligned by VLENbytes
 *                                  2. twiddle buffer size is NP * [4bytes]
 */
void qhdsp_hvx_fft_gen_twiddles_complex_achf(qhl_cfloat16_t* twiddle, uint32_t NP, uint32_t log2NP)
{
    uint32_t i, k, k1, k2;
    float complex x[NP / 2];
    float complex y[NP / 4];

    float ar, ai;

    // Generate twiddles
    // arrange in bit reversed order

    for (i = 0; i < NP / 2; i++)
    {
        k = bitrev(i, log2NP - 1);
        ar =  qhmath_cos_f((double)(k) * 2.0 * M_PI / (double)NP);
        ai = -qhmath_sin_f((double)(k) * 2.0 * M_PI / (double)NP);
        x[i] = ar + 1i * ai;
    }

    for (i = 0; i < NP / 4; i++)
    {
        k1 = bitrev((2 * i), log2NP - 1);
        k2 = bitrev(i, log2NP - 1);
        k = k1 + k2;
        ar =  qhmath_cos_f((double)(k) * 2.0 * M_PI / (double)NP);
        ai = -qhmath_sin_f((double)(k) * 2.0 * M_PI / (double)NP);
        y[i] = ar + 1i * ai;
    }

    __fp16 *twiddle_ptr = (__fp16 *)twiddle;

    for (i = 0; i < NP / 4; i++)
    {
        *twiddle_ptr++ = (__fp16)((float)1.0);      // real part
        *twiddle_ptr++ = (__fp16)((float)0.0);      // imag part
        //twiddles[4 * i + 0] = 1.0;

        *twiddle_ptr++ = (__fp16)(qhcomplex_creal_f(x[2 * i]));     // real part
        *twiddle_ptr++ = (__fp16)(qhcomplex_cimag_f(x[2 * i]));     // imag part
        //twiddles[4 * i + 1] = x[2 * i];

        *twiddle_ptr++ = (__fp16)(qhcomplex_creal_f(x[i]));      // real part
        *twiddle_ptr++ = (__fp16)(qhcomplex_cimag_f(x[i]));      // imag part
        //twiddles[4 * i + 2] = x[i];

        *twiddle_ptr++ = (__fp16)(qhcomplex_creal_f(y[i]));     // real part
        *twiddle_ptr++ = (__fp16)(qhcomplex_cimag_f(y[i]));     // imag part
        //twiddles[4 * i + 3] = y[i];
    }
}

/**
 * @brief           [HVX] Generate twiddle factors for [half precision float-point] real 1D FFT/IFFT functions.
 * @param[in,out]   twiddle1 - output buffer for storing generated twiddle factors - for N/2-point complex FFT
 * @param[in,out]   twiddle2 - output buffer for storing generated twiddle factors - for last stage
 * @param[in]       NP - number of points on which FFT/IFFT is performed
 * @param[in]       log2NP - logarithm of power 2 of number of points
 * @note
 *                  - Generated twiddle factors are used for both FFT and IFFT.
 *                  - Code doesn't utilize HVX vector instructions/parallelism.
 *                  - Assumptions:
 *                                  1. twiddle1 & twiddle2 - buffers aligned by VLENbytes
 *                                  2. twiddle1 & twiddle2 - buffer size is NP * [4bytes]
 */
void qhdsp_hvx_fft_gen_twiddles_real_achf(qhl_cfloat16_t *twiddle1, qhl_cfloat16_t *twiddle2, uint32_t np, uint32_t log2np)
{
    uint32_t i, k, k1, k2;
    float ar, ai;

    np     = np / 2;
    log2np = log2np - 1;

    __fp16 *twiddle1_ptr = (__fp16 *)twiddle1;
    __fp16 *twiddle2_ptr = (__fp16 *)twiddle2;

    // Generate twiddles for complexFFT
    // arrange in bit reversed order
    for (i = 0; i < np / 4; i++)
    {
        *twiddle1_ptr++ = (__fp16)((float)1.0);      // real part
        *twiddle1_ptr++ = (__fp16)((float)0.0);      // imag part
        //twiddle1[4 * i + 0] = 1.0;

        k2 = bitrev(2 * i, log2np - 1);
        ar =  qhmath_cos_f((double)(k2) * 2.0 * M_PI / (double)np);
        ai = -qhmath_sin_f((double)(k2) * 2.0 * M_PI / (double)np);

        *twiddle1_ptr++ = (__fp16)ar;     // real part
        *twiddle1_ptr++ = (__fp16)ai;     // imag part
        //twiddle1[4 * i + 1] = ar + 1i * ai;

        k1 = bitrev(i, log2np - 1);
        ar =  qhmath_cos_f((double)(k1) * 2.0 * M_PI / (double)np);
        ai = -qhmath_sin_f((double)(k1) * 2.0 * M_PI / (double)np);

        *twiddle1_ptr++ = (__fp16)ar;     // real part
        *twiddle1_ptr++ = (__fp16)ai;     // imag part
        //twiddle1[4 * i + 2] = ar + 1i * ai;

        k = k1 + k2;
        ar =  qhmath_cos_f((double)(k) * 2.0 * M_PI / (double)np);
        ai = -qhmath_sin_f((double)(k) * 2.0 * M_PI / (double)np);

        *twiddle1_ptr++ = (__fp16)ar;     // real part
        *twiddle1_ptr++ = (__fp16)ai;     // imag part
        //twiddle1[4 * i + 3] = ar + 1i * ai;
    }

    for (i = 1; i <= np; i++)
    {
        ar = -0.5 * qhmath_sin_f((double)(i) * M_PI / (double)np);
        ai = -0.5 * qhmath_cos_f((double)(i) * M_PI / (double)np);

        *twiddle2_ptr++ = (__fp16)ar;     // real part
        *twiddle2_ptr++ = (__fp16)ai;     // imag part
        //twiddle2[i - 1] = ar + 1i * ai;
    }
}

/**
 * @brief           [HVX] Generate twiddle factors for [half precision float-point] real 2D FFT/IFFT functions.
 * @param[in,out]   twiddle1 - output buffer for storing generated twiddle factors - for N/2-point complex FFT
 * @param[in,out]   twiddle2 - output buffer for storing generated twiddle factors - for last stage
 * @param[in]       NP - number of points on which FFT/IFFT is performed
 * @param[in]       log2NP - logarithm of power 2 of number of points
 * @note
 *                  - Generated twiddle factors are used for both FFT and IFFT.
 *                  - Code doesn't utilize HVX vector instructions/parallelism.
 *                  - Assumptions:
 *                                  1. twiddle1 & twiddle2 - buffers aligned by VLENbytes
 *                                  2. twiddle1 - buffer size is NP * [4bytes]
 *                                  2. twiddle2 - buffer size is NP/2 * [4bytes]
 */
void qhdsp_hvx_2D_fft_gen_twiddles_real_achf(qhl_cfloat16_t* twiddle1, qhl_cfloat16_t* twiddle2, uint32_t NP, uint32_t log2NP)
{
    uint32_t i, k, k1, k2;
    float complex x[NP / 2];
    float complex y[NP / 4];

    float ar, ai;

    // Generate twiddles
    // arrange in bit reversed order

    for (i = 0; i < NP / 2; i++)
    {
        k = bitrev(i, log2NP - 1);
        ar =  qhmath_cos_f((double)(k) * 2.0 * M_PI / (double)NP);
        ai = -qhmath_sin_f((double)(k) * 2.0 * M_PI / (double)NP);
        x[i] = ar + 1i * ai;
    }

    for (i = 0; i < NP / 4; i++)
    {
        k1 = bitrev((2 * i), log2NP - 1);
        k2 = bitrev(i, log2NP - 1);
        k = k1 + k2;
        ar =  qhmath_cos_f((double)(k) * 2.0 * M_PI / (double)NP);
        ai = -qhmath_sin_f((double)(k) * 2.0 * M_PI / (double)NP);
        y[i] = ar + 1i * ai;
    }

    __fp16 *twiddle_ptr = (__fp16 *)twiddle1;

    for (i = 0; i < NP / 4; i++)
    {
        *twiddle_ptr++ = (__fp16)((float)1.0);
        *twiddle_ptr++ = (__fp16)((float)0.0);
        //twiddle1[4 * i + 0] = 1.0;

        *twiddle_ptr++ = (__fp16)(qhcomplex_creal_f(x[2 * i]));
        *twiddle_ptr++ = (__fp16)(qhcomplex_cimag_f(x[2 * i]));
        //twiddle1[4 * i + 1] = x[2 * i];

        *twiddle_ptr++ = (__fp16)(qhcomplex_creal_f(x[i]));
        *twiddle_ptr++ = (__fp16)(qhcomplex_cimag_f(x[i]));
        //twiddle1[4 * i + 2] = x[i];

        *twiddle_ptr++ = (__fp16)(qhcomplex_creal_f(y[i]));
        *twiddle_ptr++ = (__fp16)(qhcomplex_cimag_f(y[i]));
        //twiddle1[4 * i + 3] = y[i];
    }

    NP = NP/2;

    twiddle_ptr = (__fp16 *)twiddle2;
    for (i = 1; i <= NP; i++)
    {
        ar = -0.5 * qhmath_sin_f((double)(i) * M_PI / (double)NP);
        ai = -0.5 * qhmath_cos_f((double)(i) * M_PI / (double)NP);

        *twiddle_ptr++ = (__fp16)ar;
        *twiddle_ptr++ = (__fp16)ai;
        //twiddle2[i - 1] = ar + 1i * ai;
    }
}
#endif
